import numpy as np
from numpy.core.records import array
import scipy.io as scio
import matplotlib.pyplot as plt
import math
from curve_line_segmentation_f import lineFilter
import io_f
from compute_f import BL2Gauss
# path_data_dir = 'F:/空天/15 车库定位/代码/MSGD/datasets/'
# save_dir = './output/site2/F1'
# filename = 'WGB2a-1_groundtruth.g2o'

# path_data_dir = 'F:/空天/15 车库定位/数据/2021.7.13室内外跑车数据/Results/'
# save_dir = './output/site2/F1'
# filename = 'final.g2o'
# path_filename = path_data_dir + filename
# path_filename = './DK.g2o'
path_filename = 'F:/空天/15 车库定位/代码/Data_Pro_py/final.g2o'
# path_filename = 'F:/空天/15 车库定位/代码/GraphSLAM_tutorials_code/g2o_test/data/sphere_after.g2o'
# path_filename = 'F:/空天/15 车库定位/代码//slambook/pose_graph/2D/INTEL.g2o'
path_data_dir = '../../数据/室内外跑车数据'
GroundTruth = '/GroundTruth/20211207.txt'
GroundTruth_path_filename = path_data_dir + GroundTruth

pos_GroundTruth = io_f.read_data_groundtruth(GroundTruth_path_filename) #0 time, 1 lat, 2 lon, 3 h, 4 ori，5 pos_delta  10hz的输出频率
for i in range(pos_GroundTruth.shape[0]): #BLH转XYZ
        # pos[i,1:4] = blh2xyz84(pos[i,1], pos[i,2], pos[i,3]) #0 time, 1 lat, 2 lon, 3 h, 4 ori，5 pos_delta, 6 magx, 7 magy, 8 magz, 9 mag_xyz
        pos_GroundTruth[i,1:3] = BL2Gauss(pos_GroundTruth[i,1] , pos_GroundTruth[i,2])
for i in range(pos_GroundTruth.shape[0]-1, -1, -1): #以第一个点作为原点
    pos_GroundTruth[i,1:4] = pos_GroundTruth[i,1:4] - pos_GroundTruth[0,1:4]

# fig = plt.figure()
# # plt.plot(pos_grid[5000:-1,4],marker=',')
# pos_GroundTruth = pos_GroundTruth[9392:-7000, :]
# plt.plot(pos_GroundTruth[:, 2], pos_GroundTruth[:, 1],marker=',')
# plt.show()
# exit()
# with open("./results/01_GroundTruth.txt","w") as file_DK:
#     for i in range(1,len(pos_GroundTruth)):
#         file_DK.write('%f %f\n'%(pos_GroundTruth[i, 2], pos_GroundTruth[i, 1]))
# file_DK.close()
# print('01_GroundTruth.txt Write Success')
# exit()
with open(path_filename) as file:
    lines = file.readlines()
file.close()
pos_grid = []
vertices = []
edges = []
for line_data in lines:
    line = line_data.split()
    if line[0] == 'VERTEX_SE2':
        tmp = [int(line[1]), float(line[2]), float(line[3]), float(line[4])]
        pos_tmp = [float(line[2]), float(line[3]), 0, math.degrees(float(line[4]))%360 ]
        pos_grid.append(pos_tmp)
        vertices.append(tmp)
    elif line[0] == 'EDGE_SE2':
         tmp = [int(line[1]), int(line[2]), float(line[3]), float(line[4]), float(line[5]), float(line[6]), float(line[7]), float(line[8]), float(line[9]), float(line[10]), float(line[11])]
         edges.append(tmp)
vertices = np.array(vertices)
# print(vertices)
edges = np.array(edges)
pos_grid = np.array(pos_grid)
pos_grid_copy = pos_grid.copy()

# fig = plt.figure()

# # plt.plot(pos_grid[5000:-1,4],marker=',')
# plt.plot(pos_grid[:, 3], marker=',')


# lineFilter(pos_grid, pos_grid_copy)
# fig = plt.figure()
# plt.axis('equal')
# # plt.plot(pos_grid[:,4],marker=',')
# plt.plot(pos_grid[:, 1],pos_grid[:, 0], marker=',')
fig = plt.figure()
plt.axis('equal')
# plt.plot(pos_grid[5000:-1,4],marker=',')
plt.plot(vertices[:, 2],vertices[:, 1], marker=',')
plt.plot(pos_GroundTruth[:, 2],pos_GroundTruth[:, 1], marker=',')

# for i in range(len(edges)):
#     id_from = edges[i,0]
#     id_to = edges[i,1]
#     plt.plot(vertices[[id_from,id_to],2],vertices[[id_from,id_to],1])

# plt.show()
# plt.pause(60)
fig = plt.figure()
plt.axis('equal')
# plt.plot(pos_grid[5000:-1,4],marker=',')
plt.plot(vertices[:, 2],vertices[:, 1], marker=',')
# plt.plot(pos_GroundTruth[:, 2],pos_GroundTruth[:, 1], marker=',')

# for i in range(len(edges)):
#     id_from = edges[i,0]
#     id_to = edges[i,1]
#     plt.plot(vertices[[id_from,id_to],2],vertices[[id_from,id_to],1])

plt.show()
scio.savemat('DK_e.mat', {'edge':edges})
scio.savemat('DK_v.mat', {'vertices':vertices})